#include "Gimble5.h"
#include <FastSerial.h>
#include <Servo.h>

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  pinMode(IMUPIN, OUTPUT); //set groundstart signal mode
  digitalWrite(IMUPIN,HIGH); //dont groundstart on default

  //init serial
  DEBUGPORT.begin(57600); 
  IMUPORT.begin(38400);
  CAMERAPORT.begin(9600);

  //timer4 setup
  TCCR4A=0;
  TCCR4B=B00000001;//prescaler=1
  TCCR4C=0;

  //servo setup
  ROLSERVO.attach(ROLPIN);
  PITSERVO.attach(PITPIN);
  PANSERVO.attach(PANPIN);
  TILSERVO.attach(TILPIN);

  PITSERVO.writeMicroseconds(PITCMDNEU);
  ROLSERVO.writeMicroseconds(ROLCMDNEU);
  TILSERVO.writeMicroseconds(TILCMDNEU);
  PANSERVO.writeMicroseconds(PANCMDNEU);  

  //int0 ISR for pan signal
  pinMode(INT0PIN, INPUT); //pan signal from APM
  digitalWrite(INT0PIN,HIGH); //internal pullup
  //EICRA|=B00000011;//rising INT0
  //EIMSK|=B00000001;//enable INT0

  //int1 ISR for tilt signal
  pinMode(INT1PIN, INPUT); //tilt signal from APM
  digitalWrite(INT1PIN,HIGH); //internal pullup
  //EICRA|=B00001100;//rising INT1
  //EIMSK|=B00000010;//enable INT1

  //ICP4 ISR for hall effect
  pinMode(ICP4PIN, INPUT); //hall effect
  digitalWrite(ICP4PIN,HIGH); //internal pullup
  //TCCR4B|=B11000000;//ICP filter plus rising
  //TIMSK4|=B00100000;//ICP enable

  //ICP4 ISR for photo1
  //pinMode(ICP4PIN, INPUT); //hall effect
  //digitalWrite(ICP4PIN,HIGH); //internal pullup
  //TCCR4B|=B11000000;//ICP filter plus rising
  //TIMSK4|=B00100000;//ICP enable

  //int4 ISR for photointerruptor one
  pinMode(INT4PIN, INPUT); //photointerruptor one signal
  digitalWrite(INT4PIN,HIGH); //internal pullup
  //EICRB|=B00000010;//falling INT4
  //EIMSK|=B00010000;//enable INT4

  //int4 ISR for hall
  //pinMode(INT4PIN, INPUT); //photointerruptor one signal
  //digitalWrite(INT4PIN,HIGH); //internal pullup
  //EICRB|=B00000010;//falling INT4
  //EIMSK|=B00010000;//enable INT4  

  //int5 ISR for photointerruptor two
  //pinMode(INT5PIN, INPUT); //photointerruptor two signal
  //digitalWrite(INT5PIN,HIGH); //internal pullup
  //EICRB|=B00001000;//falling INT5
  //EIMSK|=B00100000;//enable INT5

} 


void loop()
{
  //Serial buffers   
  byte IMUbuffer[BUFFERSIZE];
  char DEBUGbuffer[BUFFERSIZE];
  byte CAMERAbuffer[BUFFERSIZE];

  //IMU variables
  int roll;
  int pitch;
  int yaw;
  byte msgID;
  long longitude=0;
  long latitude=0;
  int altitude=0;
  int ground_speed=0;
  int ground_course=0;
  long gpstime=0;
  byte fix;

  //servo variables
  int photocmd=0;
  unsigned int rollcmd=ROLCMDNEU;
  unsigned int pitchcmd=PITCMDNEU;
  unsigned int pancmd=PANCMDNEU;
  unsigned int tiltcmd=TILCMDNEU;
  unsigned int panmeas;
  unsigned int tiltmeas;

  //intial positions
  PITSERVO.writeMicroseconds(pitchcmd);
  ROLSERVO.writeMicroseconds(rollcmd);
  TILSERVO.writeMicroseconds(tiltcmd);
  PANSERVO.writeMicroseconds(pancmd);  

  //Misc variables
  byte j=0; //counter for camerareceive
  byte light=0;
  unsigned long t=millis();
  unsigned long td=millis(); //timing for deploying
  unsigned long tr=millis(); //timing for retracting
  //unsigned long th=millis(); //timing for homing
  unsigned long tp=millis(); //timing for panning

  byte zoom=0x00; //zoom value for camera
  byte buffsize; //packet size for debug
  int debugvar;  //temporary storage
  byte deployed=0; //deployed or retracted
  byte deploying=0;
  byte retracting=0;
  //byte homing=0;
  //byte homed=0;
  byte panning=0;
  //byte kh=0; //homing counter
  unsigned int kr=0; //retracting counter
  unsigned int kd=0; //deploying counter

  while(1){

    PITSERVO.writeMicroseconds(pitchcmd);
    ROLSERVO.writeMicroseconds(rollcmd);
    TILSERVO.writeMicroseconds(tiltcmd);
    PANSERVO.writeMicroseconds(pancmd);  

    /*
    if (!digitalRead(INT4PIN)) {
     homed=1;
     photopos=0;
     }
     */

    //homing loop
    /*
    if (homing) {
     if (homed) { //if on magnet or previously saw magnet
     photocmd=0;
     homing=0;
     PANSERVO.writeMicroseconds(PANCMDNEU);
     DEBUGPORT.println("Homing Succeeded");
     } else { //not homed yet
     if ((millis()-th)>(k*1000)) {
     switch (k) {
     case 0:
     photocmd=10;
     break;
     case 1:
     photocmd=-20;
     break;
     case 2:
     photocmd=30;
     break;
     case 3:
     photocmd=-40;
     break;
     case 4:
     photocmd=0;
     break;
     default:
     homing=0;
     PANSERVO.writeMicroseconds(PANCMDNEU);
     DEBUGPORT.println("Homing Failed");
     }//end switch
     k++;
     th=millis();
     }//end timing
     } //end homed
     }//end homing
     */

    //pan measurement available
    /*
    if (flagINT0) {
     panmeas=(timer2INT0-timer1INT0)>>4; //16MHz
     panmeas=panmeas-6; //normalization (check it)
     flagINT0=0;
     //panmeas=constrain(panmeas,POSMIN,POSMAX); //disabled cause we generate pancmd
     //photocmd=map(panmeas,POSMIN,POSMAX,PHOTOMIN,PHOTOMAX);
     }
     */

    //tilt measurement available
    /*
    if (flagINT1) {
     tiltmeas=(timer2INT1-timer1INT1)>>4; //16MHz
     tiltmeas=tiltmeas-17; //normalization (check it)
     flagINT1=0;
     //tiltcmd=constrain(tiltmeas,POSMIN,POSMAX);
     //TILSERVO.writeMicroseconds(tiltcmd); //disabled cause we generate tiltcmd   
     }    
     */

    //this if statement needs to run real fast
    //to ensure we don't overshoot pan camera
    //this can be replaced by a PID loop eventually
    /*
    if ((homed||homing)&&(deployed||retracting)) {
     if ((photocmd-photopos)>PANDEAD) {
     pancmd=PANCMDMAX;
     dir=1;
     PANSERVO.writeMicroseconds(pancmd);
     } else if ((photocmd-photopos)<-PANDEAD) {
     pancmd=PANCMDMIN;
     dir=0;
     PANSERVO.writeMicroseconds(pancmd);
     } else {
     pancmd=PANCMDNEU;
     PANSERVO.writeMicroseconds(pancmd);
     }
     }
     */

    //runs if new packet is available
    if(IMUreceive(IMUbuffer)){
      msgID=IMUbuffer[1];//packet ID
      if (msgID==0x02){ //Attitude packet
        roll=bytes2int(&IMUbuffer[2]);
        pitch=bytes2int(&IMUbuffer[4]);
        yaw=bytes2int(&IMUbuffer[6]);
        roll=constrain(roll,ROLMIN,ROLMAX);
        pitch=constrain(pitch,PITMIN,PITMAX);
        if (deployed) {
          rollcmd=map(-(roll<<1),2*ROLMIN,2*ROLMAX,ROLCMDMIN,ROLCMDMAX);
          pitchcmd=map(pitch<<1,2*PITMIN,2*PITMAX,PITCMDMIN,PITCMDMAX);
        }
      } 
      if (msgID==0x03){//GPS packet
        fix=IMUbuffer[20];
        if (fix) {
          longitude=bytes2long(&IMUbuffer[2]);
          latitude=bytes2long(&IMUbuffer[6]);
          altitude=bytes2int(&IMUbuffer[10]);
          ground_speed=bytes2int(&IMUbuffer[12]);
          ground_course=bytes2int(&IMUbuffer[14]);
          gpstime=bytes2long(&IMUbuffer[16]);
        }
      }
    } //end receive event

    //receiving camera bytes, prints once a full packet
    if (CAMERAPORT.available()>0) {
      CAMERAbuffer[j++]=CAMERAPORT.read();
      if (CAMERAbuffer[j-1]==0xFF) {
        DEBUGPORT.write(CAMERAbuffer,j);
        j=0;
      }
    }

    //runs if we have a debug packet
    buffsize=DEBUGreceive(DEBUGbuffer); //size of packet    
    if (buffsize) {
      DEBUGPORT.write((byte *)DEBUGbuffer,buffsize); //echo packet
      //if (DEBUGbuffer[0]=='P' && DEBUGbuffer[1]=='A') { //set pan ex PA3000 for 30 deg
      //  debugvar=atoi(&DEBUGbuffer[2]);
      //  debugvar=constrain(debugvar,PANMIN,PANMAX);
      //  photocmd=map(debugvar,PANMIN,PANMAX,PHOMIN,PHOMAX);
      //}
      if (DEBUGbuffer[0]=='P' && DEBUGbuffer[1]=='+') { //set pan ex PA3000 for 30 deg
        tp=millis();
        panning=1;
        pancmd=PANCMDMAX;
      }
      if (DEBUGbuffer[0]=='P' && DEBUGbuffer[1]=='-') { //set pan ex PA3000 for 30 deg
        tp=millis();
        panning=1;
        pancmd=PANCMDMIN;
      }      
      //if (DEBUGbuffer[0]=='T' && DEBUGbuffer[1]=='I') { //set tilt ex TI3000! for 30deg
      //  debugvar=atoi(&DEBUGbuffer[2]);
      //  debugvar=constrain(debugvar,TILMIN,TILMAX);
      //  tiltcmd=map(debugvar,TILMIN,TILMAX,TILCMDMIN,TILCMDMAX);
      //}    
      if (DEBUGbuffer[0]=='T' && DEBUGbuffer[1]=='+') { //increment tilt
        tiltcmd+=100;
        tiltcmd=constrain(tiltcmd,TILCMDMIN,TILCMDMAX);
      }
      if (DEBUGbuffer[0]=='T' && DEBUGbuffer[1]=='-') { //decrement tilt
        tiltcmd-=100;
        tiltcmd=constrain(tiltcmd,TILCMDMIN,TILCMDMAX);
      }         
      if (DEBUGbuffer[0]=='R' && DEBUGbuffer[1]=='E') { //retract, set default pos
        deployed=0;
        retracting=1;
        tr=millis();
        rollcmd=ROLCMDNEU;
        tiltcmd=TILCMDNEU;
        //photocmd=0;
      }       
      if (DEBUGbuffer[0]=='D' && DEBUGbuffer[1]=='E') { //deploy set desired pos
        td=millis();
        deploying=1;
      }      
      if (DEBUGbuffer[0]=='G' && DEBUGbuffer[1]=='S') { //issue groundstart on imu
        digitalWrite(IMUPIN,LOW); //do a groundstart
      }
      //if (DEBUGbuffer[0]=='H' && DEBUGbuffer[1]=='O') { //home pan
      //  if (homed) {
      //    DEBUGPORT.print("Already Homed");
      //  } else if (!deployed) {  
      //      DEBUGPORT.print("Not Deployed");
      //  } else { //not homed, and deployed
      //    DEBUGPORT.print("Homing");
      //    homing=1;
      //    photopos=0;
      //    k=0;
      //    th=millis();
      //    PANSERVO.writeMicroseconds(PANCMDNEU);
      //  }
      //}
      if (DEBUGbuffer[0]=='Z' && DEBUGbuffer[1]=='+') { //increase zoom
        if (zoom!=7) zoom++;
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x47;
        CAMERAbuffer[4]=zoom;
        CAMERAbuffer[5]=0x00;
        CAMERAbuffer[6]=0x00;
        CAMERAbuffer[7]=0x00;
        CAMERAbuffer[8]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,9);
      }
      if (DEBUGbuffer[0]=='Z' && DEBUGbuffer[1]=='-') { //decrease zoom
        if (zoom!=0) zoom--;
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x47;
        CAMERAbuffer[4]=zoom;
        CAMERAbuffer[5]=0x00;
        CAMERAbuffer[6]=0x00;
        CAMERAbuffer[7]=0x00;
        CAMERAbuffer[8]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,9);          
      } 
      if (DEBUGbuffer[0]=='O' && DEBUGbuffer[1]=='N') { //turn on (usually not nessesary)
        //CAMERAbuffer[0]=0xF8;
        //CAMERAbuffer[1]=0x30;
        //CAMERAbuffer[2]=0x01;
        //CAMERAbuffer[3]=0xFF;
        //CAMERAPORT.write(CAMERAbuffer,4); // set address
        //CAMERAbuffer[0]=0x81;
        //CAMERAbuffer[1]=0x01;
        //CAMERAbuffer[2]=0x00;
        //CAMERAbuffer[3]=0x01;
        //CAMERAbuffer[4]=0xFF;
        //CAMERAPORT.write(CAMERAbuffer,5); // clear cmd buffer
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x00;
        CAMERAbuffer[4]=0x02;
        CAMERAbuffer[5]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,6); // CAM on  
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x15;
        CAMERAbuffer[4]=0x02;
        CAMERAbuffer[5]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,6); // disp on    
      }                    
      if (DEBUGbuffer[0]=='O' && DEBUGbuffer[1]=='F') { //turn off
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x00;
        CAMERAbuffer[4]=0x03;
        CAMERAbuffer[5]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,6); // CAM off  
        CAMERAbuffer[0]=0x81;
        CAMERAbuffer[1]=0x01;
        CAMERAbuffer[2]=0x04;
        CAMERAbuffer[3]=0x15;
        CAMERAbuffer[4]=0x03;
        CAMERAbuffer[5]=0xFF;
        CAMERAPORT.write(CAMERAbuffer,6); // disp off    
      }     
    }//end receive

    if (retracting && ((millis()-tr) >20)) {
      tr=millis();
      pitchcmd=pitchcmd+20;
      pitchcmd=constrain(pitchcmd,PITCMDMIN,PITCMDRETRACT);
      if (pitchcmd==2100) {
        retracting=0;
      }
      //PITSERVO.writeMicroseconds(pitchcmd);
    }
    if (deploying && ((millis()-td) > 20)) {
      td=millis();
      pitchcmd=pitchcmd-20;
      pitchcmd=constrain(pitchcmd,PITCMDMIN,PITCMDRETRACT);
      if (pitchcmd<=PITCMDNEU) {
        deploying=0;
        deployed=1;
      }
      //PITSERVO.writeMicroseconds(pitchcmd);
    }
    if (panning && ((millis()-tp) > 150)) {
      panning=0;
      pancmd=PANCMDNEU;
      //PANSERVO.writeMicroseconds(pancmd);
    }


    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      light=!light;
      digitalWrite(13, light);   // set the LED on

      DEBUGPORT.print(tiltcmd);
      DEBUGPORT.print("\t");
      //DEBUGPORT.print(retracting);
      //DEBUGPORT.print("\t");     
      //DEBUGPORT.print(deployed);
      //DEBUGPORT.print("\t");
      //DEBUGPORT.print(pitchcmd);
      //DEBUGPORT.print("\t"); 
      //DEBUGPORT.print(rollcmd);
      //DEBUGPORT.print("\t");
      //DEBUGPORT.print(pitch);
      //DEBUGPORT.print("\t");
      //DEBUGPORT.print(roll);
      //DEBUGPORT.print("\t");
      //DEBUGPORT.print(deploying);DEBUGPORT.print("\t");
      //DEBUGPORT.print(retracting);DEBUGPORT.print("\t");
      //DEBUGPORT.print(deployed);DEBUGPORT.print("\t");
      //DEBUGPORT.print(homed);DEBUGPORT.print("\t");
      //DEBUGPORT.print(homing);DEBUGPORT.print("\t");
      //DEBUGPORT.print(photocmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(photopos);DEBUGPORT.print("\t");
      //DEBUGPORT.print(dir);DEBUGPORT.print("\t");
      //DEBUGPORT.print(pancmd);DEBUGPORT.print("\t");
      //DEBUGPORT.print(deployed);DEBUGPORT.print("\t");      
      DEBUGPORT.println();

    }//end 10Hz loop

  }//end while(1)
}//end loop()


